#include "./motor.h"
#include "../compat.h"
#include "../conn/proto.h"
#include "../rtx/rtx_os.h"

/**
 * 下层电机
 * PC8 PC0
 * PC9 PC2
 * PB0
 * 
 * 上层电机
 * PC6 PC1
 * PC7 PC3
 * PC4
*/

GPIO_INFO(BOTTOM_UP_HIN, GPIOC, 8)
GPIO_INFO(BOTTOM_UP_LIN, GPIOC, 0)
GPIO_INFO(BOTTOM_DOWN_HIN, GPIOC, 9)
GPIO_INFO(BOTTOM_DOWN_LIN, GPIOC, 2)
GPIO_INFO(BOTTOM_SENSOR, GPIOD, 11)

GPIO_INFO(TOP_UP_HIN, GPIOC, 6)
GPIO_INFO(TOP_UP_LIN, GPIOC, 1)
GPIO_INFO(TOP_DOWN_HIN, GPIOC, 7)
GPIO_INFO(TOP_DOWN_LIN, GPIOC, 3)
GPIO_INFO(TOP_SENSOR, GPIOD, 5)

static void bottom_ccw() {
    BOTTOM_UP_HIN_set_high();
    BOTTOM_UP_LIN_set_high();
    BOTTOM_DOWN_HIN_set_low();
    BOTTOM_DOWN_LIN_set_low();
}

static void bottom_cw() {
    BOTTOM_UP_HIN_set_low();
    BOTTOM_UP_LIN_set_low();
    BOTTOM_DOWN_HIN_set_high();
    BOTTOM_DOWN_LIN_set_high();
}

static void bottom_stop() {
    BOTTOM_UP_HIN_set_low();
    BOTTOM_UP_LIN_set_high();
    BOTTOM_DOWN_HIN_set_low();
    BOTTOM_DOWN_LIN_set_high();
}

static void top_ccw() {
    TOP_UP_HIN_set_high();
    TOP_UP_LIN_set_high();
    TOP_DOWN_HIN_set_low();
    TOP_DOWN_LIN_set_low();
}

static void top_cw() {
    TOP_UP_HIN_set_low();
    TOP_UP_LIN_set_low();
    TOP_DOWN_HIN_set_high();
    TOP_DOWN_LIN_set_high();
}

static void top_stop() {
    TOP_UP_HIN_set_low();
    TOP_UP_LIN_set_high();
    TOP_DOWN_HIN_set_low();
    TOP_DOWN_LIN_set_high();
}

void motor_init() {
    REG_MODIEF(RCC->APB2ENR, 0, RCC_APB2ENR_IOPCEN | RCC_APB2ENR_IOPDEN);
    
    BOTTOM_UP_HIN_init(MODE_PP);
    BOTTOM_UP_LIN_init(MODE_PP);
    BOTTOM_DOWN_HIN_init(MODE_PP);
    BOTTOM_DOWN_LIN_init(MODE_PP);
    BOTTOM_SENSOR_init(MODE_IN_FLOAT);

    TOP_UP_HIN_init(MODE_PP);
    TOP_UP_LIN_init(MODE_PP);
    TOP_DOWN_HIN_init(MODE_PP);
    TOP_DOWN_LIN_init(MODE_PP);
    TOP_SENSOR_init(MODE_IN_FLOAT);

    bottom_stop();
    top_stop();
}

uint8_t motor_debug_ctrl(uint8_t id, uint8_t dir) {
    uint16_t v = (id << 8) + dir;
    switch (v) {
        case 0x0100: top_stop(); break;
        case 0x0101: top_cw(); break;
        case 0x0102: top_ccw(); break;

        case 0x0200: bottom_stop(); break;
        case 0x0201: bottom_cw(); break;
        case 0x0202: bottom_ccw(); break;

        default: return EC_INVALID;
    }
    return EC_OK;
}

static uint8_t top_state = 0; // 0 CW, 1 CCW
static uint8_t bottom_state = 0; // 0 CW, 1 CCW
#define MOTOR_TIMEOUT   10 * 1000

uint8_t top_motor_exec() {

    uint32_t time_count = 0;
    uint8_t ec = MOTOR_EC_OK;

    if (!TOP_SENSOR_read()) {
        return MOTOR_EC_INVALID;
    }

    if (top_state == 0) {
        top_cw();
    } else {
        top_ccw();
    }

    for (;;) {
        if (!TOP_SENSOR_read()) {
            break;
        }
        osDelay(10);
        time_count += 10;
        if (time_count >= MOTOR_TIMEOUT) {
            ec = MOTOR_EC_TIMEOUT;
            break;
        }
    }

    top_state = 0x01 ^ top_state;
    top_stop();

    return ec;
}

uint8_t bottom_motor_exec() {

    uint32_t time_count = 0;
    uint8_t ec = MOTOR_EC_OK;

    if (!BOTTOM_SENSOR_read()) {
        return MOTOR_EC_INVALID;
    }

    if (bottom_state == 0) {
        bottom_cw();
    } else {
        bottom_ccw();
    }

    for (;;) {
        if (!BOTTOM_SENSOR_read()) {
            break;
        }
        osDelay(10);
        time_count += 10;
        if (time_count >= MOTOR_TIMEOUT) {
            ec = MOTOR_EC_TIMEOUT;
            break;
        }
    }

    bottom_state = 0x01 ^ bottom_state;
    bottom_stop();

    return ec;
}



